#pragma once

#include <cmath>
#include <Eigen/Core>
#include <vector>

Eigen::VectorXd getPlane(std::vector<Eigen::Vector3d> points)
{
    // return: [[a], [b], [c]]
    // equation: ax + by + c = z
    int nPoints = points.size();
    Eigen::MatrixXd a;
    Eigen::VectorXd b;
    a.setZero(nPoints, 3);
    b.setZero(nPoints);

    for (int i = 0; i < nPoints; ++i)
    {
        a(i, 0) = points.at(i)(0);
        a(i, 1) = points.at(i)(1);
        a(i, 2) = 1.0;
        b(i) = points.at(i)(2);
    }

    return (a.transpose() * a).inverse() * a.transpose() * b;
}

Eigen::VectorXd getPlane(Eigen::Matrix<double, 4, 3> mat)
{
    std::vector<Eigen::Vector3d> points;
    for (int i = 0; i < 4; ++i)
    {
        points.push_back(mat.row(i).transpose());
    }
    return getPlane(points);
}
